/// by Alvaro Cuno, Fev2008

#ifndef __RBF_MESHDEF_H__
#define __RBF_MESHDEF_H__

#include "rbfFunction_3to3.h"
#include "mesh.h"
#include "gskeleton.h"

#include <iterator>

class RbfMeshDef: public RbfFunction3to3 {
   
public:
   RbfMeshDef() :
      RbfFunction3to3() {
      model = 0;
   }
   
   RbfMeshDef(Mesh *m) :
      RbfFunction3to3() {
      assert(m != 0);
      model = m;
   }
   
   /// Constructor: Deforms a mesh using a control points set
   /// @param mesh the mesh model
   /// @param centers the control points set
   RbfMeshDef(Mesh *m, vector<Point3> &centers) {
      
      assert(m != 0);
      assert(not centers.empty());
      model = m;
      setting(centers);
   }
   
   /// Constructor: Deforms a mesh using a control points set over the mesh
   /// @param mesh the mesh model
   /// @param centers the positions of control vertices
   /// @param ids identifier list of control vertices
   RbfMeshDef(Mesh *m, vector<Point3> &centers, vector<int> &ids) {
      /*
      assert(m != 0);
      assert(not centers.empty());
      model = m;
      setting(centers);
      
      cout<<" ... computing the distance table mlsmeshdeformer... "<<endl;
      dTable.resize(model->nvertices());
      for (uint i=0; i<dTable.size(); ++i) {
         dTable[i].resize(centers->size(),0.0);
      }
      
      cout<<" ... computing vertex/constraints(vertex) distances walking over the mesh surface ... "<<endl;
      /// fill the distance table. Each entry has the value [vertice][constraint]
      for (uint j=0; j<constraints->size(); ++j) {
         model->floodfill_Dijkstra(ids[j]);
         Vertex_iterator vit = model->vertices_begin();
         for (uint i=0; i<model->nvertices(); ++i, ++vit) {
            dTable[i][j] = vit->d;
         }
      }
      */
   }
   
   /// Constructor: Deforms a mesh using a skeleton structure
   /// @param mesh the mesh model
   /// @param skeleton the skeleton
   RbfMeshDef(Mesh *m, GSkeleton *skeleton) :
      RbfFunction3to3() {
      assert(m != 0);
      assert(skeleton != 0);
      model = m;
      
      ipositions.resize(model->nvertices());
      inormals.resize(model->nvertices());
      
      {
      int i=0;
      Vertex_iterator vit = model->vertices_begin();
      while (vit != model->vertices_end()) {
         const Point3 &p = vit->point();
         const Vector3 &n = vit->normal();
         ipositions[i] = p;
         inormals[i] = n;
         ++vit; ++i;
      }
      }
      
      // compute the rbf inverse matrix using joints as the control points set
      centers = skeleton->joints;
      const uint &ncenters = centers.size();
      
      /// Built the RBF matrix 
      unsigned int size = ncenters+npolyterms;
      Matrix M(size, size);
      for (unsigned int row=0; row<ncenters; ++row) {
         const Point3 &Pf = centers[row];
         skeleton->floodfill_Dijkstra(row);
         for (unsigned int col=0; col<ncenters; ++col) {
            M(row,col) = skeleton->d[col];
         }
         for (unsigned int col=ncenters; col<size; ++col) {
            M(row,col) = polynomial(Pf,col-ncenters);
         }
      }
      
      /// Fill the bottom rows of the matrix
      for (unsigned int row=ncenters; row<size; ++row) {
         for (unsigned int col=0; col<size; ++col) {
            if (col < ncenters) M(row,col) = M(col,row);
            else                M(row,col) = 0.0;
         }
      }
      
      /// Invert the matrix
      iM = i(M);
      
      /// set the evaluation point
      setEvalPoints(ipositions);
      
      /// Build the distance table
      cout<<" compute_WeightsTable using path distances ... "<<endl;
      const int &njoints = skeleton->nJoints();
      dTable.resize(model->nvertices());
      for (uint i=0; i<dTable.size(); ++i) {
         dTable[i].resize(njoints,0.0);
      }
      
      uint ii=0; Number t=0;
      Vertex_iterator vi = model->vertices_begin();
      while (vi != model->vertices_end()) {
         const Point3 &p = vi->point();
         const int &idbone = vi->closestBone; /// the closest bone
         assert(idbone != -1);
         const pair<int, int> &bone = skeleton->bones[idbone];
         const Point3 &A = skeleton->getJointPos(bone.first);
         const Point3 &B = skeleton->getJointPos(bone.second);
         const Number dbone = skeleton->getDistance(p, idbone, t);
         
         const Point3 c = B + t*(A-B); // the closest point over the bone
         const Vector3 cA(c, A);
         const Vector3 cB(c, B);
         const Number dist[2] = {sqrt(cA*cA), sqrt(cB*cB)};
         
         vector<Number> dA(njoints,0.0);
         vector<Number> dB(njoints,0.0);
         skeleton->floodfill_Dijkstra(bone.first);
         for (int j=0; j<njoints; ++j) dA[j] = skeleton->d[j];
         skeleton->floodfill_Dijkstra(bone.second);
         for (int j=0; j<njoints; ++j) dB[j] = skeleton->d[j];
         
         for (int j=0; j<njoints; ++j) {
            const Number djoint = (dA[j]<dB[j])?dist[0]:dist[1]; // distance to the closest joint
            dTable[ii][j] = dbone + djoint + min(dA[j],dB[j]);
         }
         ++vi; ++ii;
      }
   }
   
   virtual ~RbfMeshDef() {
   }
   
   void setMesh(Mesh *m) {
      assert(m != 0);
      model = m;
   }
   
   void setting(vector<Point3> &centers) {
      
      assert(model != 0);
      
      ipositions.resize(model->nvertices());
      inormals.resize(model->nvertices());
      
      int i=0;
      Vertex_iterator vit = model->vertices_begin();
      while (vit != model->vertices_end()) {
         const Point3 &p = vit->point();
         const Vector3 &n = vit->normal();
         ipositions[i] = p;
         inormals[i] = n;
         ++vit; ++i;
      }
      
      // compute the rbf inverse matrix
      compute_iM(centers);
      // set the evaluation point
      setEvalPoints(ipositions);
   }
   
   void execute_deformation(vector<Point3> &fcenters) {
      
      genWeights(fcenters);
      
      int i=0;
      Vertex_iterator vit = model->vertices_begin();
      while (vit != model->vertices_end()) {
         Point3 &p = vit->point();
         p = RbfFunction3to3::evalf(ipositions[i]);
         //~ Vector3 &n = vit->normal();
         //~ n = evalf(ipositions[i]);
         ++vit; ++i;
      }
   }
   
   void execute_deformation(GSkeleton *s) {
      
      genWeights(s->joints);
      
      int i=0;
      Vertex_iterator vit = model->vertices_begin();
      while (vit != model->vertices_end()) {
         Point3 &p = vit->point();
         p = evalf(i);
         ++vit; ++i;
      }
   }
   
   Point3 evalf(int id) {
      
      const Point3 &p = ipositions[id];
      const uint &ncenters = centers.size();
      Number rx=0, ry=0, rz=0;
      /// radial component
      for (unsigned int i=0; i<ncenters; ++i) {
         const Number &PHI = dTable[id][i];
         rx += weights[i][0]*PHI;
         ry += weights[i][1]*PHI;
         rz += weights[i][2]*PHI;
      }
      
      /// polynomial component
      /// parte das colunas de lambdas sao os coeficientes do polinomio
      /// que corresponde ao componente x, y ou z
      for (unsigned int i=0; i<npolyterms; ++i) {
         const Number &valpoly = polynomial(p, i);
         rx += weights[ncenters+i][0]*valpoly;
         ry += weights[ncenters+i][1]*valpoly;
         rz += weights[ncenters+i][2]*valpoly;
      }
      return Point3(rx, ry, rz);
   }
   
private:
   Mesh                      *model;
   vector<Point3>             ipositions; // initial vertex positions
   vector<Vector3>            inormals;   // initial vertex normal
   vector<vector<Number> >    dTable; // distance table
};

#endif 
